/*
 * @Author: your name
 * @Date: 2021-05-02 12:08:44
 * @LastEditTime: 2021-05-06 19:31:27
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \gcxz\USER\task.c
 */
#include "task.h"
#include "data.h"
#include "uart_dma.h"
#include "steer.h"
#include "light.h"
uint8 count_finish_data[3] = {0xAA,0xCF,0xFF};  // 完成计步发送完成标志
uint8 task_finish_data[3] = {0xAA,0xAF,0xFF};	// 完成任务发送完成标志
uint8 show_which = 0;                           // 根据这个变量来实现oled屏幕翻页
static void updateEncoder(); // 更新编码器数据，task.cpp内部使用
float copy_chassis_target_x;
void countAndStop();		 // 延时计步并停止

void controlTask(void){
    updateEncoder();	    // 读取编码器寄存器并赋值给chassis.motor
    chassis.control();      // 解算得到各个电机目标值，并控制电机

}

void edgeBoardTask(void){
	if(couandstop_flag){                                                        // couandstop_flag由上位机控制
		couandstop_flag = 0;
		countAndStop();  
		//uart_putbuff(uart4,&count_finish_data[0],sizeof(count_finish_data));		// 计步并且停车
		sendUart_DMA(UART4,count_finish_data,3);    // TODO:换成DMA发送
		Bee(1,500);
		systick_delay_ms(2);
	}
	else if(adjust_flag){                                                       // adjust_flag 由上位机控制
		adjust_flag = 0;
		// 调用调整函数
		chassis.adjust();
        systick_delay_ms(200);
        chassis.setMotorPwm(1000);
	}
	else if(task_flag){                                                         // task_flag 由上位机控制
		task_flag = 0;
		switch(task_type){
			case 1:pingyi(1500); chassis.target_pose.x = copy_chassis_target_x;				break;//平移函数
			case 2:clipBox(); chassis.target_pose.x = copy_chassis_target_x;		break;
			case 3:raiseFlags(which_city);Chengchi_green();	systick_delay_ms(3000); putDownFlags(which_city); chassis.target_pose.x = copy_chassis_target_x;break;
			case 4:throwBall();	chassis.target_pose.x = copy_chassis_target_x;	break;
			case 5:shootTarget();chassis.target_pose.x = copy_chassis_target_x;	break;
		}
		sendUart_DMA(UART4,task_finish_data,3);//发送任务完成标志
		systick_delay_ms(2);
        Bee(1,500);
	} 
    else if(eb_dubug_flag){
        eb_dubug_flag = 0;
        switch (which_argument)
        {
        case 1: chassis.target_pose.x = argument_value*10; //当远处检测到地标时修改下位机速度为argument*10
            break;
        
        default:
            break;
        }
    }
		
	
		
	
	
	
	
	
}

void upMonitorTask(void){
//    UART_SendData1(pid_angle_out);
	Sent_Contorl();
}
//想要实现按一下同一个按键会翻页，按不同按键会调整值，不按的话就停留在变量那里
void debugTask(void){
	static int show_which = 0;
    int tmp_value = Key_Select();
	static int value;
    if(tmp_value != 6){
        value = tmp_value;
        show_which ++;
    }
    else{
        value = 5;
    }
    switch(value){
        case 5: 
                if(show_which == 0 ){
                    
                    OLED_Fill(0x00);//清屏幕
                    OLED_P6x8Str(1,1,(uint8*)"key_val:");OLED_Print_Num1(60,1,5);
                    OLED_P6x8Str(1,2,(uint8*)"cur_p_x:");OLED_Print_Num1(60,2,chassis.current_pose.x);
                    OLED_P6x8Str(1,3,(uint8*)"cur_p_y:");OLED_Print_Num1(60,3,chassis.current_pose.y);
                    OLED_P6x8Str(1,4,(uint8*)"cur_p_z:");OLED_Print_Num1(60,4,chassis.current_pose.z);
                    OLED_P6x8Str(1,5,(uint8*)"tar_p_x:");OLED_Print_Num1(60,5,chassis.target_pose.x);
                    OLED_P6x8Str(1,6,(uint8*)"tar_p_y:");OLED_Print_Num1(60,6,chassis.target_pose.y);
                    OLED_P6x8Str(1,7,(uint8*)"tar_p_z:");OLED_Print_Num1(60,7,chassis.target_pose.z);
                }
                else if(show_which == 1 ){
                    OLED_Fill(0x00);//清屏幕
                    OLED_P6x8Str(1,1,(uint8*)"key_val:");OLED_Print_Num1(60,1,5);
                    OLED_P6x8Str(1,2,(uint8*)"co_flag:");OLED_Print_Num1(60,2,control_flag);
                    OLED_P6x8Str(1,3,(uint8*)"eb_flag:");OLED_Print_Num1(60,3,edgeBoard_flag);
                    OLED_P6x8Str(1,4,(uint8*)"up_flag:");OLED_Print_Num1(60,4,upMonitor_flag);
                    OLED_P6x8Str(1,5,(uint8*)"adjust:");OLED_Print_Num1(60,5,adjust_flag);
                    OLED_P6x8Str(1,6,(uint8*)"task_type:");OLED_Print_Num1(60,6,task_flag);
                    OLED_P6x8Str(1,7,(uint8*)"which_city:");OLED_Print_Num1(60,7,which_city);
                    
                }
                else if(show_which == 2 ){
                    OLED_Fill(0x00);//清屏幕
                    OLED_P6x8Str(1,1,(uint8*)"key_val:");OLED_Print_Num1(60,1,5);
                    OLED_P6x8Str(1,2,(uint8*)"xxxx_kp:");OLED_Print_Num1(60,2,chassis.pose_pid[0].Kp);
                    OLED_P6x8Str(1,3,(uint8*)"xxxx_kd:");OLED_Print_Num1(60,3,chassis.pose_pid[0].Kd);
                    OLED_P6x8Str(1,4,(uint8*)"yyyy_kp:");OLED_Print_Num1(60,4,chassis.pose_pid[1].Kp);
                    OLED_P6x8Str(1,5,(uint8*)"yyyy_kd:");OLED_Print_Num1(60,5,chassis.pose_pid[1].Kd);
                    OLED_P6x8Str(1,6,(uint8*)"zzzz_kp:");OLED_Print_Num1(60,6,chassis.pose_pid[2].Kp);
                    OLED_P6x8Str(1,7,(uint8*)"zzzz_kd:");OLED_Print_Num1(60,7,chassis.pose_pid[2].Kd);
                }
                else if(show_which == 3 && tmp_value != 0){
                    show_which = 0;
                }
    }
    
    //OLED_Print_Num1(3,3,chassis.current_pose.z);
	//uart_putbuff(uart4,&task_finish_data[0],sizeof(task_finish_data));
    // OLED_Print_Num1(60,1,flag);

}

void updateEncoder(){

	chassis.motors[RF].updateSpeed(ftm_quad_get(ftm2));
	ftm_quad_clean(ftm2);

	chassis.motors[LF].updateSpeed(-ftm_quad_get(ftm1)); // 正负号依据轮子的位置
	ftm_quad_clean(ftm1);

	chassis.motors[LB].updateSpeed(LB_cnt);
	LB_cnt = 0;
	
	chassis.motors[RB].updateSpeed(RB_cnt);
	RB_cnt = 0;
}

/**
* @brief  延时计步并停止
* @param  None.
* @return None.
*/
void countAndStop(){		  
	chassis.setMotorZero();
	Bee(1,500);							//bee了0.5秒
}


void pingyi(int time){
    chassis.testPinYi(1);//向左
    systick_delay_ms(time);
    chassis.setMotorZero();
    Suying_Red();
    chassis.testPinYi(-1);//向右
    systick_delay_ms(time);
    chassis.setMotorZero();

}
